import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
import launch
import launch_ros.actions
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode, ParameterFile
from nav2_common.launch import RewrittenYaml

def generate_launch_description():
    # Parameters
    lifecycle_nodes = ['map_server']
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    autostart = True

    # Get the path to the configuration file
    config = os.path.join(get_package_share_directory('nav2_map_server'), 'launch', 'ft_map_server_setup.yaml')

    # Nodes launching commands
    start_map_server_cmd = launch_ros.actions.Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[config])

    start_lifecycle_manager_cmd = launch_ros.actions.Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager',
        output='screen',
        emulate_tty=True,  # Emulate TTY for better output readability
        parameters=[{'use_sim_time': use_sim_time},
                    {'autostart': autostart},
                    {'node_names': lifecycle_nodes}])

    ld = LaunchDescription()

    # Add the nodes to the launch description
    ld.add_action(start_map_server_cmd)
    ld.add_action(start_lifecycle_manager_cmd)

    # Optionally, declare launch arguments
    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) time if true')
    ld.add_action(declare_use_sim_time_cmd)

    return ld